#include <iostream>
#include <fstream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/common/eigen.h>
#include <pcl/common/centroid.h>

using namespace std;

void help(char *appname) {
    printf("计算平面方程，用 A, B, C, D 表示\n");
    printf("Usage: %s <option> <input_file.txt> <out_folder>\n", appname);
}

/// 读取txt文件然后转成cloud内容，计算数据
int main(int argc, char** argv)
{
    if (argc < 4) {
        help(argv[0]);
        return -1;
    }

    int option = atoi(argv[1]);
    string inputPointsTxt = argv[2];
    string outputFolder = argv[3];

    // 创建输出目录
    if (0 != access(outputFolder.c_str(), W_OK)) {
        mkdir(outputFolder.c_str(), 0777);
    }

    if (outputFolder[ outputFolder.size() -1 ] != '/' ) {
        outputFolder = outputFolder + "/";
    }

    // 读取txt中内容，放到cloud中
    double x, y, z;
    double r, g, b;

    FILE *fpInput = fopen(inputPointsTxt.c_str(), "r");
    if (fpInput == nullptr) {
        cout << "Fail to open file " << inputPointsTxt << endl;
        return -1;
    }
    int n = 0;
    while (6 == fscanf(fpInput, "%lf %lf %lf %lf %lf %lf\n", &x, &y, &z, &r, &g, &b)) {
        n++;
    }

    // 读取文件
    pcl::PointCloud<pcl::PointXYZ> cloud;
    cloud.width = n;
    cloud.height = 1;
    cloud.is_dense = false;
    cloud.points.resize(cloud.width * cloud.height);
    //新建一个点云文件，然后将结构中获取的xyz值传递到点云指针cloud中。

    int i = 0;
    rewind(fpInput);
    while (6 == fscanf(fpInput, "%lf %lf %lf %lf %lf %lf\n", &x, &y, &z, &r, &g, &b)) {
        cloud.points[i].x = x; // 转成m
        cloud.points[i].y = y;
        cloud.points[i].z = z;
//        cloud.points[i].r = r;
//        cloud.points[i].g = g;
//        cloud.points[i].b = b;
//        cloud.points[i].a = 1; // 没弄清楚为什么a要设为1
        ++i;
    }
    fclose(fpInput);

    // 计算法向量
    Eigen::Vector4d centroid;                    // 质心
    Eigen::Matrix3d covariance_matrix;           // 协方差矩阵
    // 计算归一化协方差矩阵和质心
    pcl::computeMeanAndCovarianceMatrix(cloud, covariance_matrix, centroid);
    // 计算协方差矩阵的特征值与特征向量
    Eigen::Matrix3d eigenVectors;
    Eigen::Vector3d eigenValues;
    pcl::eigen33(covariance_matrix, eigenVectors, eigenValues);
    // 查找最小特征值的位置
    Eigen::Vector3d::Index minRow, minCol;
    eigenValues.minCoeff(&minRow, &minCol);
    // 获取平面方程：AX+BY+CZ+D = 0的系数
    Eigen::Vector3d normal = eigenVectors.col(minCol);
    double D = -normal.dot(centroid.head<3>());

    cout << "平面模型系数为：\n"
         << "A=" << normal[0] << "\n"
         << "B=" << normal[1] << "\n"
         << "C=" << normal[2] << "\n"
         << "D=" << D << "\n" << endl;


    fstream f(outputFolder + "plane1.txt", ios::out);//供写使用，文件不存在则创建，存在则清空原内容
    f << normal[0] <<" "<<normal[1] << " " << normal[2] << D; //写入数据
    f.close();

    return 0;

}
